/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Accelerometer;

import Wasa.Acceleration;
import java.sql.Time;
import java.util.ArrayList;
import java.util.HashMap;

/**
 *
 * @author gianmariospacagna
 */
public class Velocity {

    public HashMap<Time, Acceleration> accelerationSamples;
    protected double x;
    protected double y;
    protected double z;
    protected Acceleration prevAcc;
    protected Acceleration initAcc;
    private long time;
    private long prevTime;

    public Velocity(Acceleration initAcc) {
        this.x = 0;
        this.y = 0;
        this.z = 0;
        this.initAcc = initAcc;
        this.prevAcc = initAcc;
        this.time = initAcc.getTime();
    }
    
    public Velocity (){
        this.x = 0;
        this.y = 0;
        this.z = 0;
        this.time = System.currentTimeMillis();
        this.prevTime=0;
    }

    /* first integral technique*/
    public void updateVelocity2(Acceleration acc) {
        //System.out.println("Update Velocity");

        double deltaT = acc.getTime() - prevAcc.getTime();
        deltaT /= 1000;
        //System.out.println("deltaT="+deltaT);
        double deltaX = (acc.getX() + prevAcc.getX() - (2 * initAcc.getX())) * deltaT * 0.5;
        /*System.out.println("NewAccX: "+acc.getX());
        System.out.println("PrevAccX: "+prevAcc.getX() );
        System.out.println("InitAccX: "+initAcc.getX());
        System.out.println("deltaX="+deltaX);*/
        this.x += deltaX;
        double deltaY = (acc.getY() + prevAcc.getY() - (2 * initAcc.getY())) * deltaT * 0.5;
        //System.out.println("deltaY="+deltaY);
        this.y += deltaY;
        this.z += (acc.getZ() + prevAcc.getZ() - 2 * initAcc.getZ()) * deltaT * 0.5;
        prevAcc = acc;
        this.time = acc.getTime();
    }

    public void updateVelocity(Acceleration acc) {
        prevTime=time;
        time=acc.getTime();
        
        double deltaT = time - prevTime;
        deltaT /= 1000;
        double deltaX = acc.getX() * deltaT;
        double deltaY = acc.getY() * deltaT;
        double deltaZ = acc.getZ() * deltaT;
        this.x+=deltaX;
        this.y+=deltaY;
        this.z+=deltaZ;
        
        
    }

    public double getX() {
        return x;
    }

    public double getY() {
        return y;
    }

    public double getZ() {
        return z;
    }

    public long getTime() {
        return time;
    }
}
